#include "lib.h"

static char* hexChar = "0123456789ABCDEF";

inline static char 
convertNibleHex(unsigned char b){
    return hexChar[b];
}

void *
memset (void *s, char c, unsigned long n)
{
    unsigned long n4 = n >> 2;
    void *start = s;

    //always  set 4 Bytes at once
    if(n4) {
        register unsigned int pattern = c ? (c << 24 | c << 16 | c << 8 | c) : 0;
        register unsigned int *p = s;
        register int i = (n4 + 0x0f) >> 4;

        //unroll loop
        switch(n4 & 0x0f) {
            case 0: do { *p++ = pattern;
            case 15:     *p++ = pattern;
            case 14:     *p++ = pattern;
            case 13:     *p++ = pattern;
            case 12:     *p++ = pattern;
            case 11:     *p++ = pattern;
            case 10:     *p++ = pattern;
            case 9:      *p++ = pattern;
            case 8:      *p++ = pattern;
            case 7:      *p++ = pattern;
            case 6:      *p++ = pattern;
            case 5:      *p++ = pattern;
            case 4:      *p++ = pattern;
            case 3:      *p++ = pattern;
            case 2:      *p++ = pattern;
            case 1:      *p++ = pattern;
                    } while(--i > 0);
        }

        start = p;
        n = n & 3;
    }

    //copy remaining bytes
    //ifs generate very optimized code
    if(n&2) {
        *((unsigned short*)start) = c | c << 8;
        start = (void*)((unsigned long)start + 2);
    } 
    if(n&1){
        *((unsigned char*)start) = c;
    }

    return s;
}

void 
memcpy(void *t, const void *src, unsigned long size) 
{
    unsigned long size4 = size >> 2;
    unsigned char *newT = (unsigned char*)t;
    unsigned char *newS = (unsigned char*)src;

    if(size4){
        register unsigned long *t4 = (unsigned long*)t;
        register unsigned long *s4 = (unsigned long*)src;
        register int i = (size4 + 0x0f) >> 4;
        switch(size4 & 0x0f){
            case 0: do { *t4++ = *s4++;
            case 15:     *t4++ = *s4++;
            case 14:     *t4++ = *s4++;
            case 13:     *t4++ = *s4++;
            case 12:     *t4++ = *s4++;
            case 11:     *t4++ = *s4++;
            case 10:     *t4++ = *s4++;
            case 9:      *t4++ = *s4++;
            case 8:      *t4++ = *s4++;
            case 7:      *t4++ = *s4++;
            case 6:      *t4++ = *s4++;
            case 5:      *t4++ = *s4++;
            case 4:      *t4++ = *s4++;
            case 3:      *t4++ = *s4++;
            case 2:      *t4++ = *s4++;
            case 1:      *t4++ = *s4++;
                    } while(--i > 0);
        }

        newT = (unsigned char*)t4;
        newS = (unsigned char*)s4;
        size = size & 3;
    }

    //copy remaining bytes
    //ifs generate very optimized code
    if(size&2) {
        *((unsigned short*)newT) = *((unsigned short*)newS);
        newT = (unsigned char*)((unsigned long)newT + 2);
        newS = (unsigned char*)((unsigned long)newS + 2);
    } 
    if(size&1){
        *newT = *newS;
    }
}

char *
strcpy(char *d, const char *s)
{
    char    *rval = d;

    while((*d++ = *s++));
    return(rval);
}

char *
strncpy(char *d, const char *s, int len)
{
    char    *rval = d;

    while(len-- > 0 && (*d++ = *s++));
    while(len-- > 0)    *d++ = 0;
    return(rval);
}

int 
strcmp(const char *a, const char *b)
{
    while (*a && *b && (*a == *b)) {
        a++;
        b++;
    }

    return(*a - *b);
}


int 
strncmp(const char *a, const char *b, int n)
{
    while (--n > 0 && *a && *b && (*a == *b)) {
        a++;
        b++;
    }

    return(*a - *b);
}


int 
strlen(const char *s)
{
    register int    n;

    for(n = 0; *s++; n++);
    return(n);
}

int 
atoi(const char *s)
{
    unsigned val = 0;
    int      neg = 0;

    while (*s == ' ' || *s == '\t')  s++; // ignore beginning white space 
    if    (*s == '-')                s++, neg = 1;
    while (*s >= '0' && *s <= '9')   val = 10 * val + (*s++ - '0'); //convert integer value

    return neg? -(int)val : (int)val;
}
/********* ab hier************/
static void 
__putchar(char c, char** str){
    **str = c;
    (*str)++;
}

static int 
__putdec(int n, int base, char** str)
{
    int new_base = base * 10;

    if(n >= new_base) n = __putdec(n, new_base, str);

    char a;
    if(n >= 0){
        for(a = '0' - 1; n >= 0; n-=base){
            ++a;
        }
    } else {
        for(a = '9' + 1; n < 0; n+= base) --a;
    }

    __putchar(a, str);
    return n;
}

//for base 16 and 8
static void 
__putother(unsigned int n, char** str, int base){
    int show = 0;
    int shift = base == 16 ? 28 : 30;
    int bits = base == 16 ? 4 : 3;
    base--; //create mask

    for(; shift >= 0; shift-=bits){
        unsigned int i = (n >> shift) & base;
        if(i > 0 || show || !shift){
            __putchar( convertNibleHex(i), str);
            show = 1;
        }
    }
}

//base can be 10, 16 or 8
static int 
unsigned_to_str(char *s, int maxlen, unsigned value, int base)
{
    char field[32];                            /* Zwischenspeicher                             */
    char *orig_s = s;                          /* zur Berechnung der konvertierten Zeichen     */
    char *limit = s + maxlen;                  /* zeigt hinter das letzte benutzbare Zeichen   */
    char *f = field;

    if (value == 0){                           /* bei Wert == 0 ist die Konvertierung simpel   */
        if (maxlen > 0) {
            *s = '0';
            return 1;
        } else return 0;
    } 
    if(base == 10){
        int n = value;
        if(n >= 10){
            n = __putdec(value, 10, &f);
            n = n < 0 ? 10 + n : n;
        }
        __putchar(n + '0', &f);
    } else __putother(value, &f, base);

    //copy number string
    char *tf = field;
    while(tf < f && s < limit)
        *s++ = *tf++;

    return s - orig_s;                                                  /* Anzahl der Zeichen zurueckgeben              */
}

static void 
pad_string(char **dest, char *last, int count, int filler)
{
    char *s = *dest;

    while (count > 0 && s < last)
    {
        *s++ = filler;
        count--;
    }
    *dest = s;
}

int 
vsnprintf(char *dest, int maxlen, const char *format, int *argp)
{
    char field[32];                                           /* zum Formatieren der Zahlen                   */
    char *orig_dest = dest;                                           /* zur Berechnug der Zeichenanzahl              */
    char *last = dest + (maxlen - 1);                                     /* letzte benutzbare Speicherstelle             */
    char c;                                             /* zum Speichern eines char-Parameters          */
    char *s;                                              /* Laufvariable                                 */
    int len;                                              /* temp. Variable                               */
    int width;                                              /* Breite der Ausgabe                           */
    int d;                                              /* zum Speichern eines int-Parameters           */
    int u;                                              /* zum Speichern eines unsigned-Parameters      */
    int left_justify;                                           /* linksbuendige Ausgabe                        */
    int filler;                                             /* Zeichen zur buendigen Ausgabe                */
    int alternate;                                            /* Alternative Darstellung (man -s 3s printf)   */
    int use_sign;                                             /* immer Vorzeichen drucken                     */

    while (*format)
    {
        switch (*format)
        {
            case '%':
                format++;                                           /* Formatanweisung ueberspringen                */

                if (*format == '#')                                         /* alternative Darstellungsform                 */
                {
                    alternate = 1;
                    format++;
                }
                else
                    alternate = 0;
                if (!*format)                                           /* Abruch der Schleife                          */
                    continue;

                if (*format == '-')                                         /* linksbuendige Ausgabe                        */
                {
                    left_justify = 1;
                    format++;
                }
                else
                    left_justify = 0;
                if (!*format)                                           /* Abruch der Schleife                          */
                    continue;

                if (*format == '+')                                         /* immer Vorzeichen ausgeben                    */
                {
                    use_sign = 1;
                    format++;
                }
                else
                    use_sign = 0;
                if (!*format)                                           /* Abruch der Schleife                          */
                    continue;

                if (*format == '0')                                         /* fuehrende Nullen fuer's padding              */
                {
                    filler = '0';
                    format++;
                }
                else
                    filler = ' ';
                if (!*format)                                           /* Abruch der Schleife                          */
                    continue;

                if (*format >= '0' && *format <= '9')                           /* Breite der Ausgabe                           */
                {
                    width = *format - '0';
                    format++;
                    while (*format >= '0' && *format <= '9')
                    {
                        width = width * 10 + (*format - '0');
                        format++;
                    }
                }
                else
                    width = 0;

                if (!*format)                                           /* Abbruch der Schleife                         */
                    continue;

                switch (*format)
                {
                    case 'c':                                                     /* character                                    */
                        c = (char) *argp;
                        argp++;

                        width--;
                        if (!left_justify)
                            pad_string(&dest, last, width, ' ');
                        if (dest < last)
                            *dest++ = c;
                        if (left_justify)
                            pad_string(&dest, last, width, ' ');
                        break;

                    case 's':                                                     /* string                                       */
                        s = (char *) *argp;
                        argp++;
                        len = strlen(s);
                        width -= len;

                        if (!left_justify)
                            pad_string(&dest, last, width, ' ');
                        while (*s && dest < last)
                            *dest++ = *s++;
                        if (left_justify)
                            pad_string(&dest, last, width, ' ');
                        break;

                    case 'd':                                                     /* signed integer                               */
                    case 'i':
                        d = (int) *argp;
                        argp++;
                        if (dest < last)
                        {
                            if (d < 0)
                            {
                                *dest++ = '-';
                                d = -d;
                                width--;
                            }
                            else if (use_sign)
                            {
                                *dest++ = '+';
                                width--;
                            }

                            len = unsigned_to_str(field, sizeof(field) - 1, d, 10);
                            field[len] = '\0';
                            width -= len;
                            if (!left_justify)
                                pad_string(&dest, last, width, filler);
                            s = field;
                            while (*s && dest < last)
                                *dest++ = *s++;
                            if (left_justify)
                                pad_string(&dest, last, width, filler);
                        }
                        break;

                    case 'u':                                                     /* unsigned integer                             */
                        u = (unsigned int) *argp;
                        argp++;
                        len = unsigned_to_str(field, sizeof(field) - 1, u, 10);
                        field[len] = '\0';
                        width -= len;
                        if (!left_justify)
                            pad_string(&dest, last, width, filler);
                        s = field;
                        while (*s && dest < last)
                            *dest++ = *s++;
                        if (left_justify)
                            pad_string(&dest, last, width, filler);
                        break;

                    case 'o':                                                     /* integer, Oktaldarstellung                    */
                        u = (int) *argp;
                        argp++;
                        len = unsigned_to_str(field, sizeof(field) - 1, u, 8);
                        field[len] = '\0';
                        width -= len;
                        if (alternate)
                            width--;
                        if (!left_justify)
                            pad_string(&dest, last, width, filler);
                        if (alternate)
                            if (dest < last)
                                *dest++ = '0';
                        s = field;
                        while (*s && dest < last)
                            *dest++ = *s++;
                        if (left_justify)
                            pad_string(&dest, last, width, filler);
                        break;

                    case 'p':                                                     /* pointer                                      */
                        filler = '0';
                        width = 10;
                        alternate = 1;                                              /* FALLTHRU!!!                                  */

                    case 'x':                                                     /* integer, Hexdarstellung                      */
                        d = (int) *argp;
                        argp++;
                        len = unsigned_to_str(field, sizeof(field) - 1, d, 16);
                        field[len] = '\0';
                        width -= len;
                        if (alternate)
                        {
                            width -= 2;
                            if (dest < last)
                                *dest++ = '0';
                            if (dest < last)
                                *dest++ = 'x';
                        }
                        if (!left_justify)
                            pad_string(&dest, last, width, filler);
                        s = field;
                        while (*s && dest < last)
                            *dest++ = *s++;
                        if (left_justify)
                            pad_string(&dest, last, width, filler);
                        break;

                    default:                                                      /* alle anderen Formatangaben einfach kopieren, */
                        if (dest < last)                                            /* z.B. bei `%%'                                */
                            *dest++ = *format;
                        break;
                }
                break;

            default:                                                          /* Zeichen ohne `%' normal kopieren             */
                if (dest < last)
                    *dest++ = *format;
                break;
        }
        format++;
    }
    *dest++ = '\0';                                                       /* String terminieren                           */

    return dest - orig_dest;                                              /* Anzahl der konvertierten Zeichen zurueck     */
}

